/*
 * Author - Lukasz Baranski <barluk87@gmail.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 */

#include <inttypes.h>
#include <avr/io.h>

#include <avr/interrupt.h>
#include <avr/wdt.h> 
#include <string.h>
#include <stdio.h>
#include <avr/sleep.h>
#include "frsky.h"
#include "uart.h"

#define FILTERLEN 8

volatile unsigned long bufor=0, fifo[16]; //filteredFifo[16];

volatile int deltaFifo[4];

volatile int temp;

volatile unsigned char overSample=0, fifoPointer=0, deltaFifoPtr=0, initComp=0, sampled=0, tick=0;

enum currentState {INIT, VARIO};

ISR(ADC_vect){

   temp =(int)ADC;

   if(temp<0){                 //temp=temp/64; ADLAR = 1;
           temp= temp>>6;      //
           temp|=(0xFC<<8);
       }else{
           temp= temp>>6;
       }

   temp+=512; //?
   bufor += temp;            //    + 4-bit ADC
   overSample++;

   if(overSample == 0xff){            //    0,05mV/bit > 20 bit/mV
                                       // ~37,5Hz f sample
                                      // 12,5 bit/m
        bufor = bufor>>4;
        fifo[fifoPointer] = bufor;
        bufor=0;

       //first free place
        fifoPointer++;
        if(fifoPointer == 16){
            fifoPointer=0;
            initComp=1;
            }

       sampled=1;
    }

       //to another mesurment
       /*
       *   change mux[ptr]
       *   start conv
       */
}

ISR(TIMER0_OVF_vect){ //61Hz
    tick=1;

}

void ADC_Init(){
	ADMUX|=(1<<3)|(1<<0)|(1<<ADLAR);  //10x GAIN; Vref 4096mV;9-bit/Vref
	ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADIE)|(7<<0);// 125kHz for 16Mhz clock 

}

void Timer0_init(){
    TIMSK|=(1<<TOIE0);
    TCCR0|=(5<<1);
}

long altitude(){
    long suma = 0;
    unsigned char i;
    for(i=0; i<16; i++){
       suma += fifo[i];
    }
    return (suma);
}

int varioAverage(int Vspeed){ //to test
    
    deltaFifo[deltaFifoPtr] = Vspeed;
    deltaFifoPtr++;
    
    if(deltaFifoPtr==4)
        deltaFifoPtr=0;
    
    int vario_tmp=0;
    unsigned char i;
    for(i=0;i<4;i++){
       vario_tmp+=deltaFifo[i];
       }

    return vario_tmp/4;
}

//FILE uart_str = FDEV_SETUP_STREAM(sendByte, NULL, _FDEV_SETUP_RW); 

int main(){

	//DDRC=0xFF;
	//PORTC=0xFF;

	initUart();
	//stdout = &uart_str;

    ADC_Init();
    
	sei();
	wdt_enable(WDTO_1S);
    set_sleep_mode(SLEEP_MODE_ADC);

	enum currentState state = INIT;
    unsigned short cnt = 0;
    long suma, old, height, ground=0;
	int dhp;
	int dh;
    int varioFiltered;
    
    clearData();
    
    while(1){
    switch(state){
        case INIT:
			if(initComp == 1){
				if(sampled ==1 ){
					sampled = 0;
	                cnt++;			
				}
			}
            if(cnt == 50){
                state = VARIO;
                suma = altitude();
                ground = suma;
				cnt = 0;
            }
        break;

        case VARIO:
            if(sampled == 1){
    			sampled = 0;
                if(cnt == 9){  //time interval ~250ms
                    old = suma;
                    suma = altitude();
                    height = (ground-suma)/200; //(12,5*16) [m]
                    dhp = (old - suma)/50;  //[m/s]
                    dh = ((old - suma)%50)*2; // [cm/s]
                    varioFiltered = varioAverage((int)(dhp*10+(dh/10)));            
					setAltitude((int)height);
					setVspd(varioFiltered);
					sendFrame();
					cnt = 0;
                }
                cnt++;
			}
            //if reset sw go to init
        break;

    }
        wdt_reset();
		sleep_enable();
    }

}
